extern "C" {
#include <som_pak/som_devrobs.h>
#include <som_pak/lvq_pak.h>
#include <som_pak/som_rout.h>
#include <som_pak/datafile.h>
#include <som_pak/labels.h>
};

#include "ros/ros.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float32MultiArray.h"
#include "ros_som_pak/TrainSom.h"

class SomPakNode 
{
public:
	SomPakNode();
	void saveCodebook();
	void close();
private:
	ros::NodeHandle n;	
	ros::Subscriber MapSubscriber_;
	ros::Subscriber InvMapSubscriber_;
	ros::Publisher MapPublisher_;
	ros::Publisher InvMapPublisher_;
	ros::ServiceServer service;
	std::string in_code_file;
	std::string out_code_file;
	
	struct data_entry *input;
	struct data_entry *output;
	struct entries *codes_;
	struct entries *data_;
	eptr p;
	struct teach_params_counters *teach_params_;
	int *coords;
	int *last_coords;
	int *error;
	double alpha;
	double radius;
	int rlen;
	int ewin;
	int dim;
	int x;
	int y;
	
	bool updateNewData(ros_som_pak::TrainSom::Request  &req,
					   ros_som_pak::TrainSom::Response &res );
	void mapNewData(const std_msgs::Float32MultiArray::ConstPtr& array);
	void inverseMap(const std_msgs::Float32MultiArray::ConstPtr& value);
};

SomPakNode::SomPakNode(){
	service = n.advertiseService("teach_som", &SomPakNode::updateNewData, this);
	
	MapSubscriber_ = n.subscribe<std_msgs::Float32MultiArray>("data", 1, boost::bind(&SomPakNode::mapNewData,this,_1));
	MapPublisher_ = n.advertise<std_msgs::Float32MultiArray>("state", 100);
	
	InvMapSubscriber_ = n.subscribe<std_msgs::Float32MultiArray>("inv_state", 1, boost::bind(&SomPakNode::inverseMap,this,_1));
	InvMapPublisher_ = n.advertise<std_msgs::Float32MultiArray>("inv_data", 100);
	
	ros::NodeHandle nh("~");
	
	n.param("dim", dim, 90);
	n.param("x", x, 2);
	n.param("y", y , 50);
	n.param("alpha", alpha,  0.02);
	n.param("radius", radius, 15.0);
	n.param("rlen", rlen, 5000);
	n.param("ewin", ewin, 25);
	nh.param<std::string>("in_code_file", in_code_file, "bcodebook.cod");
	nh.param<std::string>("out_code_file", out_code_file, "bcodebook.cod");
	
	codes_ = NULL;
	
	float points[dim];
	
	set_globals();
	init_random(33);
	
	//define the data size
	data_ = init_dataset(dim);
	
	codes_ = open_entries((char*)in_code_file.c_str());
	
	if(codes_ == NULL || codes_->dimension != dim) 
	{
		ROS_INFO("Could not open the proper codebook data file %s, so using random init",in_code_file.c_str());
		int i;
		
		//create a "vector for a data entry"
		for (i = 0; i < dim; i++) points[i] = (rand()%1);
		
		input = make_data_entry(points);
		addto_dataset(data_, input);
		
		
		codes_ = randinit_codes(data_, TOPOL_HEXA, NEIGH_BUBBLE, x, y);
	}
	else 
	{
		ROS_INFO("Using existing codebook data from file %s",in_code_file.c_str());
		codes_ = read_entries(codes_);
	}
	
	//set up teaching parameters based on the code book
	teach_params_ = construct_teach_params(codes_, LINEAR, LINEAR);
	
	init_training_session(teach_params_, alpha, radius, rlen, ewin);
}

void SomPakNode::close()
{
	close_entries(codes_);
	//close_entries(data);
	// cannot close data because points# and mask are constant size arrays
	
	if (data_) free_dataset(data_);
	if (teach_params_) free_teach_params(teach_params_);
}

void SomPakNode::saveCodebook()
{
	int error = save_entries(codes_, (char*)out_code_file.c_str());
	if(error) 
		ROS_INFO("  could not create file %s\n", out_code_file.c_str());
	else 
		ROS_INFO("  resulting codebook file saved to %s\n", out_code_file.c_str());
}

bool SomPakNode::updateNewData(ros_som_pak::TrainSom::Request  &req,
							   ros_som_pak::TrainSom::Response &res )
{
	//----------------First Teach------------------------------------	
	int i;
	//float TeachArr[req.Instruction.data.size()];
	int counter = (int)req.Instruction.data.size()/dim;
	//req.Instruction.stride[1] == counter?
	
	ROS_INFO("Number of values to train for is %d.",counter);
	std::vector<float>::const_iterator it1 = req.Instruction.data.begin();
	for (int cnt = 0; cnt < counter; cnt++) 
	{
		i = 0;
		float TeachArr[dim];
		
		for( ; i < dim && it1 != req.Instruction.data.end() ; ++it1)
		{
			TeachArr[i] = (float)*it1;
			i++;
		}
		
		input = make_data_entry(TeachArr);
		
		last_coords = (int*)malloc(2*sizeof(int));  
		last_coords[0] = last_coords[1] = -1; 
		
		coords = train_one(teach_params_, input, last_coords, 1);
		
		free(last_coords);                                 
		last_coords = coords;                              
	}
	//----------------Now test---------------------------------------	
	counter = (int)req.TestSet.data.size()/dim;
	//req.TestSet.stride[1] == counter?

	ROS_INFO("Number of values to map for is %d.",counter);
	for (int cnt = 0; cnt < counter; cnt++)
	{
		i = 0;
		float MapArr[dim];
		std::vector<float>::const_iterator it2 = req.TestSet.data.begin();
		for( ; i < dim && it2 != req.TestSet.data.end(); ++it2)
		{
			MapArr[i] = (float)*it2;
			i++;
		}
		input = make_data_entry(MapArr);
		
		last_coords = (int*)malloc(2*sizeof(int));  
		last_coords[0] = last_coords[1] = -1; 
		
		coords = map_one(teach_params_, input, last_coords, 1);
		
		free(last_coords);                                 
		last_coords = coords;                              
		
		res.Results.data.push_back(coords[0]*x+coords[1]);
	}
	return true;
}

void SomPakNode::mapNewData(const std_msgs::Float32MultiArray::ConstPtr& array)
{
	int i = 0;
	std_msgs::Float32MultiArray j;
	float myArr[array->data.size()];
	
	for(std::vector<float>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
	{
		myArr[i] = *it;
		i++;
	}
	
	ROS_INFO("Number of values to map for is %d.",(int)array->data.size()/dim);
	
	input = make_data_entry(myArr);
	
	last_coords = (int*)malloc(2*sizeof(int));  
	last_coords[0] = last_coords[1] = -1; 
	
	coords = map_one(teach_params_, input, last_coords, 1);
	
    free(last_coords);                                 
    last_coords = coords;                              
	
	j.data.push_back(coords[0]*x+coords[1]);
	//Publish array
	MapPublisher_.publish(j);
	return;
}

void SomPakNode::inverseMap(const std_msgs::Float32MultiArray::ConstPtr& value)
{
	int location[2];
	int index;
	int values[value->data.size()];
	std_msgs::Float32MultiArray array;
	
	index = 0;
	
	for(std::vector<float>::const_iterator it = value->data.begin(); it != value->data.end(); ++it)
	{
		values[index] =(int)*it;
		index++;
	}
	
	ROS_INFO("Number of values to determine correspondence for is %d.",(int)value->data.size());
	
	{
		index = 0;
		if (values[index] < 0) 
		{
			ROS_INFO("Inverse map value of %d is too small.", values[index]);
			for (int i = 0; i < dim; i++)
			{
				array.data.push_back(INFINITY);
			}
			
		}
		else if (values[index] > (x*y - 1)) 
		{
			ROS_INFO("Inverse map value of %d is too large.", values[index]);
			for (int i = 0; i < dim; i++)
			{
				array.data.push_back(INFINITY);
			}
		}
		else
		{ 
			location[0]= (values[index] % x);
			location[1]= (values[index] / x);
			
			output = get_model_vector(codes_, location);
			if ((output->points) == NULL) 
			{
				ROS_INFO("Points not found for map %d",values[index]);
				for (int i = 0; i < dim; i++)
				{
					array.data.push_back(INFINITY);
				}				
			}
			else 
			{
				for (int i = 0; i < dim; i++)
				{
					array.data.push_back(output->points[i]);
				}
			}
			
		}
	}
	//Publish array
	InvMapPublisher_.publish(array);			
	
}


int main(int argc, char **argv)
{	
	ros::init(argc, argv, "SomPakNode");
	
	SomPakNode p;
	
	ros::Rate loop_rate(60);
	
	while (ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();
	}
	p.saveCodebook();
	p.close();
	return 0;
}

